#include "pid.h"

tPid pidA, pidB;
tPid pidHW_Tracking; //红外循迹的PID

void PID_init() {
    memset(((void *) (&pidA)), 0, sizeof(pidA));
    memset(((void *) (&pidB)), 0, sizeof(pidB));
    memset(((void *) (&pidHW_Tracking)), 0, sizeof(pidHW_Tracking));
    pidA.Kp = 16;
    pidA.Ki = 6;
    pidA.Kd = 1;
    pidB.Kp = 16;
    pidB.Ki = 6;
    pidB.Kd = 1;
    pidHW_Tracking.Kp = 1.5;
    pidHW_Tracking.Ki = 1;
    pidHW_Tracking.Kd = 0;
}

/**************************************************************************
P 当前误差	比例控制算法
I 累积误差	积分控制算法
D 当前误差-上次误差	微分控制算法
**************************************************************************/
float P_realize(tPid *pid, float actual_val) {
    pid->actual_val = actual_val;    // 传递真实值
    pid->err = pid->target_val - pid->actual_val;    // 计算当前误差
    // 比例控制调节 当前值=Kp*当前误差
    pid->actual_val = pid->Kp * pid->err;

    return pid->actual_val;
}


float PI_realize(tPid *pid, float actual_val) {
    pid->actual_val = actual_val;    // 传递真实值
    pid->err = pid->target_val - pid->actual_val;    // 计算当前误差
    pid->err_sum += pid->err;    // 累积误差

    // 比例控制调节 当前值=Kp*当前误差
    pid->actual_val = pid->Kp * pid->err \
 + pid->Ki * pid->err_sum;

    return pid->actual_val;
}


/**
 *         PID
 * @param pid          要设置的pid
 * @param actual_val   电机速度的真实值
 * @return
 */
float PID_realize(tPid *pid, float actual_val) {
    pid->actual_val = actual_val;    // 传递真实值
    pid->err = pid->target_val - pid->actual_val;    // 计算当前误差
    pid->err_sum += pid->err;    // 累积误差

    // 比例控制调节 当前值=Kp*当前误差
    pid->actual_val = pid->Kp * pid->err \
 + pid->Ki * pid->err_sum \
 + pid->Kd * (pid->err - pid->err_last);
    pid->err_last = pid->err;

    return pid->actual_val;
}
